#-------------------------------------------------------------------------------
# Name:        模块1
# Purpose:
#
# Author:      Youan
#
# Created:     01-05-2021
# Copyright:   (c) Youan 2021
# Licence:     <your licence>
#-------------------------------------------------------------------------------

import math
from functools import reduce
import queue
from collections import deque
from typing import List

RadiusEarth = 6372795.477598    #半径二次中心
#RadiusEarth = 6378137    #地球半长轴半径

def calcTrueHeading(lon1, lat1, lon2, lat2):
    """
    # 目的：依据点1和点2的经纬度，计算两点真北朝向
    # 参数：
        point p1(lon1, lat1)
        point p2(lon2, lat2)
    # 返回值：两个GPS坐标点的真北方位角
    """
    # 将十进制度数转化为弧度
    lon1, lat1, lon2, lat2 = map(math.radians, [lon1, lat1, lon2, lat2])
    dLon = lon2 - lon1
    y = math.sin(dLon) * math.cos(lat2)
    x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dLon)
    head = math.degrees(math.atan2(y, x))
    return (head + 360) % 360

def calcDistanceHaversine(lon1, lat1, lon2, lat2):
    """
    # 目的： 根据haversine公式计算两个以十进制度数表示的GPS坐标点的大圆距离
    # 参数：
        point p1(lon1, lat1)
        point p2(lon2, lat2)
    # 返回值：两个GPS坐标点的大圆距离，单位为 米
    """
    # 将十进制度数转化为弧度
    lon1, lat1, lon2, lat2 = map(math.radians, [lon1, lat1, lon2, lat2])
    # haversine公式
    dlon = lon2 - lon1
    dlat = lat2 - lat1
    a = math.sin(dlat/2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon/2)**2
    c = 2 * math.asin(math.sqrt(a))
    return c * RadiusEarth
    pass

def calcTargetPosition(lon1, lat1, heading, distance):
    """
    # 目的： 已知起点经纬度和航向（方位）、航程（距离），求终点经纬度
    # 参数：
        起点经纬度: p(lon, lat)
        航向（方位）: heading(顺时针从北)
        航程（距离）: distance
    # 返回值：终点经纬度
    # 参考：https://blog.csdn.net/diaoyanbian8143/article/details/102364521
            https://blog.csdn.net/qq_37588817/article/details/96289211
            https://blog.csdn.net/diaoyanbian8143/article/details/102364521
        lat2 = asin(sin(lat1)*cos(d/R) + cos(lat1)*sin(d/R)*cos(θ))
        lon2 = lon1 + atan2(sin(θ)*sin(d/R)*cos(lat1), cos(d/R)−sin(lat1)*sin(lat2))
        其中，
        asin = arc sin()
        d = 航程/距离。任意单位均可
        R = 地球赤道半径，单位同上 6378137m
        角距离(弧度)δ: d / R
        atan2(a,b) = arc tan(b/a)
        θ是方位角(顺时针从北)
    """
    # 将十进制度数转化为弧度
    lon1, lat1, head = map(math.radians, [lon1, lat1, heading])
    dR = distance / RadiusEarth
    lat2 = math.asin( math.sin(lat1) * math.cos(dR) + math.cos(lat1) * math.sin(dR) * math.cos(head))
    lon2 = lon1 + math.atan2( math.sin(head) * math.sin(dR) * math.cos(lat1), math.cos(dR) - math.sin(lat1) * math.sin(lat2))
    return math.degrees(lon2), math.degrees(lat2)


def convertionLonlatToXY(lonlatO, lonlatP):
    """
    # 目的：依据参照点，实现经纬度到x-y之间的转换
    # 参数：
        lonlatO：坐标原点对应的经纬度
        lonlatP：需要转换的坐标点经纬度
    # 返回值：参考坐标下的x, y
    """
    distX = calcDistanceHaversine(lonlatO[0], lonlatO[1], lonlatP[0], lonlatO[1])
    if (lonlatO[0] > lonlatP[0]):
        distX = -distX
    distY = calcDistanceHaversine(lonlatO[0], lonlatO[1], lonlatO[0], lonlatP[1])
    if (lonlatO[1] > lonlatP[1]):
        distY = -distY
    return (distX, distY)


pool = []
'''
# 关于内存缓冲区的设计讨论
# 方法1
pool = []

维持一个数据缓冲池
缓冲池的大小设置后固定，例如为10，即len(pool) = 10，表示记录最近10个时刻的系统状态数据
每个时刻的系统状态数据包括其中每条船的状态,这里表示为 [{$ship1$}, {$ship2$}, {$ship3$}, ...]
那么这个缓冲池在运行时内部的结构如下所示：

    pool = [
        [{$ship1$}, {$ship2$}, {$ship3$}, ...],
        [{$ship1$}, {$ship2$}, {$ship3$}, ...],
        [{$ship1$}, {$ship2$}, {$ship3$}, ...],
        ...
        [{$ship1$}, {$ship2$}, {$ship3$}, ...],
    ]

其中 {$ship1$} 示例:
{
    "Time": 120,
    "type": 15,
    "mmsi": "2130001,
    "length": 22.0,
    "width": 7.0,
    "sName": "Ferry0.48704",
    "lon": 118.60009834252573,
    "lat": 31.939050966970306,
    "head": 241.63576226204026,
    "rot": 0.0,
    "speed": 3.876,
    "next": 33,
    "nextLon": 118.62957717009711,
    "nextLat": 31.947045209517565
}

127.0.0.1:6379> KEYS *
1) "1#118.62941325819098#31.94717879295201"
2) "1#118.55834271890366#31.8948681764121"
3) "1#118.74309030820089#32.11444901979738"
127.0.0.1:6379> GET "1#118.62941325819098#31.94717879295201"
"{"type": 15, "mmsi": 10010, "length": 22.0, "width": 7.0, "sName": "Ferry0.48704", "lon": 118.62941325819098, "lat": 31.94717879295201, "head": 75.62487059972796, "rot": 0.0, "speed": 3.876, "next": 33, "nextLon": 118.62957717009711, "nextLat": 31.947045209517565}"
127.0.0.1:6379>


# 方法2
trace = {}

维持一个字典
字典中的每个键 为当前系统中的船舶mmsi
每个键的值是一个长度固定的列表，例如10，其意义是最近10个时刻该船的状态
如果某个时刻某个船不再出现，它将被从此字典中剔除
这个字典在运行时内部结构如下所示：

trace = {
    "s2130001": [{}, {}, ...],
    "s2130002": [],
    ...
}
'''

class Poolold():
    def __init__(self, maxlen: int = 10):
        self.MAXLEN = maxlen
        self.pool = deque(maxlen = maxlen)
        self.length = len(self.pool)
        pass

    def put(self, obj) -> bool:
        '''
        ## obj: Object
        ## 向数据池中添加一个元素，并标记为最新
        ## 如果数据池容量已经达到最大值，则删除最旧的元素
        '''
        while len(self.pool) >= self.MAXLEN:
            self.pool.popleft()
        self.pool.append(obj)
        return True

    def getLast(self, num: int = 1) -> list:
        '''
        ## num: int
        ## 获取数据池中的后num个元素，并顺序返回一个列表
        ## 如果num大于数据池中实际元素的数量，则返回实际数量的元素
        '''
        tmp = []
        if num > len(self.pool):
            num = len(self.pool)
        while num:
            tmp.append(self.pool.pop())
            num -= 1
        tmp = tmp.reverse()
        if tmp == None:
            tmp = []
        for item in tmp:
            self.pool.append(item)
        return tmp

    def length(self) -> int:
        '''
        ## 返回数据池实际的元素数量
        '''
        return self.length

class Pool():
    def __init__(self, maxlen: int = 10):
        self.MAXLEN = maxlen
        self.pool = []
        self.length = len(self.pool)
        pass

    def put(self, obj) -> bool:
        '''
        ## obj: Object
        ## 向数据池中添加一个元素，并标记为最新
        ## 如果数据池容量已经达到最大值，则删除最旧的元素
        '''
        while len(self.pool) >= self.MAXLEN:
            self.pool.pop(0)
        self.pool.append(obj)
        return True

    def getLast(self, num: int = 1) -> list:
        '''
        ## num: int
        ## 获取数据池中的后num个元素，并顺序返回一个列表
        ## 如果num大于数据池中实际元素的数量，则返回实际数量的元素
        '''
        if  len(self.pool) == 0:
            return []
        if num > len(self.pool):
            num = len(self.pool)
        return list(self.pool[-num:len(self.pool)])

    def length(self) -> int:
        '''
        ## 返回数据池实际的元素数量
        '''
        return self.length

class TSimShipBase():
    # 智能体：船舶
    # 考虑加入船舶共享基础函数：如坐标变换，自动航迹
    # 初始条件：航迹，AISInfoDyn等

    def __init__(self, Data = {}):
        # Data：初始条件，字典格式，内容包括静态信息和航迹等
        #super().__init__(SimScene, Data)
        #self.type = EntityType.SHIP
        self.auto = True
        # self.stop = False
        self.stop = False

        # Data 为字典
        ### 初始化船舶数据
        self.id = Data.get('mmsi', 0)
        self.length = Data.get('length', 0.0)
        self.width = Data.get('width', 0.0)
        self.lon = Data.get('lon', 0.0)
        self.lat = Data.get('lat', 0.0)
        self.head = Data.get('head', 60.0)
        self.rot = Data.get('rot', 0.0)    # 船舶转向角速度
        self.speed = Data.get('speed', 0.0)
        self.type = Data.get('type', 15)
        # Tracks 为航线，N个坐标点组成
        self.track = Data.get('track', [])
        self.track.insert(0, (self.lon, self.lat))
        self.next = 0 # Track中的位置
        #self.AISInfoDyn = TAISInfo()
        self.initData(Data)
        # 测试用
        self.__timeTick = 0

    def initData(self, Data = {}):
        pass

    def saveData(self):
        pass

    def Run(self):
        self.OnTimer()
        if self.next == len(self.track):
            self.next = -1
            self.lon, self.lat = self.track[self.next]
        distPoint = self.track[self.next]
        #print('Time:', self.__timeTick, ' mmsi:', self.id,
        #        ' lon:', self.lon, ' lat:', self.lat, ' head:', self.head,
        #        ' GotoPosition:', self.next, self.track[self.next],
        #        ' distance: ', calcDistanceHaversine(self.lon, self.lat, distPoint[0], distPoint[1]), 'm')
        self.__timeTick = self.__timeTick + 1
        return self.saveData()

    def getTime(self):
        return self.__timeTick

    def OnTimer(self):
        #super().OnTimer()
        if self.auto:
            # 自动模式下；
            # 驾驶决策：根据当前各项因素计算如果没有干扰。计算正常/恢复正常参数
            self.NavigateModel()
            # 避碰决策，调整设定新的head和rot等参数
            self.AntiCollision()
            pass

        # 根据水动力参数，前进一步
        if not self.stop:
            self.ShipMotionModel()

        # 检查是否出现碰撞等强制停船意外

        if self.stop:
            self.stopSimulation()
            return
        pass

    def isReachPoint(self, lon, lat, offset = 0.001):
        #return (abs(self.lon - lon) <= offset) and (abs(self.lat - lat) <= offset)
        return calcDistanceHaversine(self.lon, self.lat, lon, lat) < self.speed + offset 
        pass

    def NavigateModel(self):
        # 驾驶决策：根据当前各项因素计算如果没有干扰，依据预设航线前进所需要的正常/恢复正常参数
        # 直接修改 head, speed, rot等
        # 本处采用简单决策，如果需要更精确的计算，可在子类中继承重写即可

        # 首先检查是否到达拐点范围，如果是，以下一拐点为目标，调整head，这里考虑匀速运动
        # 如果没有下一拐点（到达终点），则本船阶段仿真结束，船舶消失
        nextPoint = self.track[2]
        (targetLon, targetLat) = nextPoint
        if self.isReachPoint(targetLon, targetLat):
            # 到达拐点，获取下一拐点
            self.next = self.next + 1
            (targetLon, targetLat) = nextPoint

            # 检查是否到达终点，如果是，则本船阶段仿真结束
            if self.next == len(self.track):
                # 终点抵达，设置仿真结束标志
                self.stop = True

        # 以拐点为目标，调整head，这里仅仅考虑匀速运动，故而 speed 和 rot 不变
        # 由于大地圆球影响，不能继续沿用之前的head参数
        self.head = calcTrueHeading(self.lon, self.lat, targetLon, targetLat)
        pass

    def AntiCollision(self):
        # data = pool[-1]
        # for ship in data:
        #     mmsi = ship['mmsi']
        #     mmsi = ship.get('mmsi', '21000')
        #     pass

        # 避碰决策，调整设定新的head和rot等参数
        # 直接修改 head, speed, rot等
        pass

    def ShipMotionModel(self):
        # 根据水动力参数，前进一步，修改新的lon,lat位置等参数
        # 默认：线性模型，如果需要更精确详细的计算，可参考水动力学模型后，在子类中继承重写即可
        # lon, lat: 起始坐标
        # speed: 航速，待统一转换，初步单位为 m/s
        # heding: 航向角，以真北为基准顺时针度量到航向线的角度
        # distance：本周期内，船舶行走的距离长度，初步单位为米
        # 返回值：新的坐标点

        # 一个周期后的行进距离，由于仿真间隔固定时刻为 1 秒，所以
        distance = self.speed # 单位为米

        # 不带船舶自主操纵函数 # 需要修正转换为经纬度
        self.lon, self.lat = calcTargetPosition(self.lon, self.lat, self.head, distance)
        pass

    def stopSimulation(self):
        if self.stop:
            # 本船仿真结束处理，船舶消失
            # save data
            # unregister self
            pass
        pass

# TODO:
#   1、融入SimBase，补充相关代码
#   2、新写SimShip子类，带有随机路径偏移
#   3、新建SimSence子类，用于 test
#   4、数据生成类





